#include <kuai/misc/xyz.hpp>
#include <kuai/mol/pbc.hpp>
#include <kuai/sim/simjob.hpp>
#include <kuai/misc/geometry.hpp>

#ifndef _KUAI_SIM_ENERGY_HPP_
#define _KUAI_SIM_ENERGY_HPP_

namespace kuai {

	class EnergyFunction
	{
	public:
		explicit EnergyFunction(Index rank0 = 1)
			: rank(rank0)
		{  }
		virtual ~EnergyFunction()
		{ }
		virtual void addE(SimulationEnvironment& env, RealNumber e[]) = 0;
		virtual void addEG(SimulationEnvironment& env, RealNumber e[], XYZ gred[]) = 0;
		virtual void addEGH(SimulationEnvironment& env, RealNumber e[], XYZ gred[], RealNumber* hess[]) = 0;
		virtual void addHX(SimulationEnvironment& env, const RealNumber x[], RealNumber result[]) = 0;

		virtual void start(SimulationEnvironment& env)
		{ }
		virtual void finish(SimulationEnvironment& env)
		{ }
		virtual void finalize(SimulationEnvironment& env)
		{ }

		inline static void addHessianItem(const HessianItem& item, Index iAtom1, Index iAtom2, RealNumber* hess[]) {
			Index offset1 = iAtom1 * 3;
			Index offset2 = iAtom2 * 3;
			for (Index i1 = 0; i1 < 3; ++i1) {
				for (Index i2 = 0; i2 < 3; ++i2) {
					hess[offset1+i1][offset2+i2] += item(i1, i2);
				}		
			}
		};
		inline static void addHessianItem(const HessianItem& item, Index iAtom1, Index iAtom2, const RealNumber x[], RealNumber result[]) {
			Index offset1 = iAtom1 * 3;
			for (Index i1 = 0; i1 < 3; ++i1) {
				for (Index i2 = 0; i2 < 3; ++i2) {
					result[offset1+i1] += item[i1][i2] * x[offset1+i1]; // hess[offset1+i1][offset2+i2] += item(i1, i2);
				}		
			}
		};

		inline static void subHessianItem(const HessianItem& item, Index iAtom1, Index iAtom2, RealNumber* hess[]) {
			Index offset1 = iAtom1 * 3;
			Index offset2 = iAtom2 * 3;
			for (Index i1 = 0; i1 < 3; ++i1) {
				for (Index i2 = 0; i2 < 3; ++i2) {
					hess[offset1+i1][offset2+i2] -= item(i1, i2);
				}		
			}
		};

		inline static void subHessianItem(const HessianItem& item, Index iAtom1, Index iAtom2, const RealNumber x[], RealNumber result[]) {
			Index offset1 = iAtom1 * 3;
			for (Index i1 = 0; i1 < 3; ++i1) {
				for (Index i2 = 0; i2 < 3; ++i2) {
					result[offset1+i1] -= item[i1][i2] * x[offset1+i1]; // hess[offset1+i1][offset2+i2] += item(i1, i2);
				}		
			}
		};

		Index rank;
	};

	class BasicForceFieldEnergy
		: public EnergyFunction
	{
	public:
		explicit BasicForceFieldEnergy(Index rank0 = 1)
			: EnergyFunction(rank0)
		{ 
			nlist = 0;
			lists = NULL;
			pars = NULL;
			coords = NULL;
			pbc = NULL;
		}

		size_t			nlist;
		Index*			lists;
		RealNumber*		pars;
		XYZ*			coords;
		PBC*			pbc;
	};

	template<typename Functor, bool usePBC>
		class FunctionBond
			: public BasicForceFieldEnergy
	{
	public:
		virtual void addE(SimulationEnvironment& env, RealNumber e[]) {
			assert (nlist % 3 == 0);
			SizePair range = env.range(nlist / 3);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*3];
				Index atom2 = lists[i*3+1];
				Index iPar  = lists[i*3+2];
				XYZ delta(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta);
				}
				e[0] += func(abs(delta), pars+iPar);
			}
		}
		virtual void addEG(SimulationEnvironment& env, RealNumber e[], XYZ gred[]) {
			assert (nlist % 3 == 0);
			SizePair range = env.range(nlist / 3);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*3];
				Index atom2 = lists[i*3+1];
				Index iPar  = lists[i*3+2];
				XYZ delta(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta);
				}
				RealNumber r = abs(delta);
				RealNumber dedr;
				e[0] += func(r, pars+iPar, dedr);
				delta *= (dedr/r);
				gred[atom1] += delta;
				gred[atom2] -= delta;
			}
		}
		virtual void addEGH(SimulationEnvironment& env, RealNumber e[], XYZ gred[], RealNumber* hess[]) {
			assert (nlist % 3 == 0);
			SizePair range = env.range(nlist / 3);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*3];
				Index atom2 = lists[i*3+1];
				Index iPar  = lists[i*3+2];
				XYZ delta(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta);
				}
				XYZ dx[1];
				HessianItem dx2[2];
				RealNumber r = calcAtomPair(delta, dx, dx2);

				RealNumber dedr, d2edr2;
				e[0] += func(r, pars+iPar, dedr, d2edr2);
				dx2[0] *= dedr;

				dx2[1] = prod(dx[0], dx[0]);
				dx2[1] *= d2edr2;

				dx[0] *= dedr;
				gred[atom1] += dx[0];
				gred[atom2] -= dx[0];

				dx2[0] += dx2[1];
				env.lock();
				addHessianItem(dx2[0], atom1, atom1, hess);
				subHessianItem(dx2[0], atom1, atom2, hess);
				subHessianItem(dx2[0], atom2, atom1, hess);
				addHessianItem(dx2[0], atom2, atom2, hess);
				env.free();
			}
		}
		virtual void addHX(SimulationEnvironment& env, const RealNumber x[], RealNumber result[]) {
			assert (nlist % 3 == 0);
			SizePair range = env.range(nlist / 3);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*3];
				Index atom2 = lists[i*3+1];
				Index iPar  = lists[i*3+2];
				XYZ delta(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta);
				}
				XYZ dx[1];
				HessianItem dx2[2];
				RealNumber r = calcAtomPair(delta, dx, dx2);

				RealNumber dedr, d2edr2;
				func(r, pars+iPar, dedr, d2edr2);
				dx2[0] *= dedr;

				dx2[1] = prod(dx[0], dx[0]);
				dx2[1] *= d2edr2;
				dx2[0] += dx2[1];
				addHessianItem(dx2[0], atom1, atom1, x, result);
				subHessianItem(dx2[0], atom1, atom2, x, result);
				subHessianItem(dx2[0], atom2, atom1, x, result);
				addHessianItem(dx2[0], atom2, atom2, x, result);
			}
		}

		Functor func;
	};

	template<typename Functor, bool usePBC>
		class FunctionAngle
			: public BasicForceFieldEnergy
	{
	public:
		virtual void addE(SimulationEnvironment& env, RealNumber e[]) {
			assert (nlist % 4 == 0);
			SizePair range = env.range(nlist / 4);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*4];
				Index atom2 = lists[i*4+1];
				Index atom3 = lists[i*4+2];
				Index iPar  = lists[i*4+3];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
				}
				RealNumber a = angle(delta1, delta2);
				e[0] += func(a, pars+iPar);
			}
		}
		virtual void addEG(SimulationEnvironment& env, RealNumber e[], XYZ gred[]) {
			assert (nlist % 4 == 0);
			SizePair range = env.range(nlist / 4);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*4];
				Index atom2 = lists[i*4+1];
				Index atom3 = lists[i*4+2];
				Index iPar  = lists[i*4+3];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
				}
				XYZ dadx[2];
				RealNumber angle = calcAngle(delta1, delta2, dadx);
				RealNumber dedr;
				e[0] += func(angle, pars+iPar, dedr);
				dadx[0] *= dedr; dadx[1] *= dedr;
				gred[atom1] += dadx[0];
				gred[atom2] -= dadx[0];
				gred[atom2] -= dadx[1];
				gred[atom3] += dadx[1];
			}
		}
		virtual void addEGH(SimulationEnvironment& env, RealNumber e[], XYZ gred[], RealNumber* hess[]) {
			assert (nlist % 4 == 0);
			SizePair range = env.range(nlist / 4);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*4];
				Index atom2 = lists[i*4+1];
				Index atom3 = lists[i*4+2];
				Index iPar  = lists[i*4+3];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
				}
				XYZ dadx[2];
				HessianItem dx2[8];
				RealNumber angle = calcAngle(delta1, delta2, dadx, dx2);
				RealNumber dedr, d2edr2;
				e[0] += func(angle, pars+iPar, dedr, d2edr2);
				
				dx2[0] *= dedr;
				dx2[1] *= dedr;
				dx2[2] *= dedr;
				dx2[3] *= dedr;

				dx2[4] = prod(dadx[0], dadx[0]); dx2[4] *= d2edr2;
				dx2[5] = prod(dadx[0], dadx[1]); dx2[5] *= d2edr2;
				dx2[6] = prod(dadx[1], dadx[0]); dx2[6] *= d2edr2;
				dx2[7] = prod(dadx[1], dadx[1]); dx2[7] *= d2edr2;

				dx2[0] += dx2[4];
				dx2[1] += dx2[5];
				dx2[2] += dx2[6];
				dx2[3] += dx2[7];

				dadx[0] *= dedr; dadx[1] *= dedr;
				gred[atom1] += dadx[0];
				gred[atom2] -= dadx[0];
				gred[atom2] -= dadx[1];
				gred[atom3] += dadx[1];

				env.lock();
				addHessianItem(dx2[0], atom1, atom1, hess);

				subHessianItem(dx2[0], atom1, atom2, hess);
				subHessianItem(dx2[1], atom1, atom2, hess);

				addHessianItem(dx2[1], atom1, atom3, hess);

				subHessianItem(dx2[0], atom2, atom1, hess);
				subHessianItem(dx2[2], atom2, atom1, hess);

				addHessianItem(dx2[0], atom2, atom2, hess);
				addHessianItem(dx2[1], atom2, atom2, hess);
				addHessianItem(dx2[2], atom2, atom2, hess);
				addHessianItem(dx2[3], atom2, atom2, hess);

				subHessianItem(dx2[1], atom2, atom3, hess);
				subHessianItem(dx2[3], atom2, atom3, hess);

				addHessianItem(dx2[2], atom3, atom1, hess);

				subHessianItem(dx2[3], atom3, atom2, hess);
				subHessianItem(dx2[2], atom3, atom2, hess);

				addHessianItem(dx2[3], atom3, atom3, hess);
				env.free();
			}
		}

		virtual void addHX(SimulationEnvironment& env, const RealNumber x[], RealNumber result[]) {
			assert (nlist % 4 == 0);
			SizePair range = env.range(nlist / 4);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*4];
				Index atom2 = lists[i*4+1];
				Index atom3 = lists[i*4+2];
				Index iPar  = lists[i*4+3];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
				}
				XYZ dadx[2];
				HessianItem dx2[8];
				RealNumber angle = calcAngle(delta1, delta2, dadx, dx2);
				RealNumber dedr, d2edr2;
				func(angle, pars+iPar, dedr, d2edr2);
				
				dx2[0] *= dedr;
				dx2[1] *= dedr;
				dx2[2] *= dedr;
				dx2[3] *= dedr;

				dx2[4] = prod(dadx[0], dadx[0]); dx2[4] *= d2edr2;
				dx2[5] = prod(dadx[0], dadx[1]); dx2[5] *= d2edr2;
				dx2[6] = prod(dadx[1], dadx[0]); dx2[6] *= d2edr2;
				dx2[7] = prod(dadx[1], dadx[1]); dx2[7] *= d2edr2;

				dx2[0] += dx2[4];
				dx2[1] += dx2[5];
				dx2[2] += dx2[6];
				dx2[3] += dx2[7];

				addHessianItem(dx2[0], atom1, atom1, x, result);

				subHessianItem(dx2[0], atom1, atom2, x, result);
				subHessianItem(dx2[1], atom1, atom2, x, result);

				addHessianItem(dx2[1], atom1, atom3, x, result);

				subHessianItem(dx2[0], atom2, atom1, x, result);
				subHessianItem(dx2[2], atom2, atom1, x, result);

				addHessianItem(dx2[0], atom2, atom2, x, result);
				addHessianItem(dx2[1], atom2, atom2, x, result);
				addHessianItem(dx2[2], atom2, atom2, x, result);
				addHessianItem(dx2[3], atom2, atom2, x, result);

				subHessianItem(dx2[1], atom2, atom3, x, result);
				subHessianItem(dx2[3], atom2, atom3, x, result);

				addHessianItem(dx2[2], atom3, atom1, x, result);

				subHessianItem(dx2[3], atom3, atom2, x, result);
				subHessianItem(dx2[2], atom3, atom2, x, result);

				addHessianItem(dx2[3], atom3, atom3, x, result);
			}
		}

		Functor func;
	};

	template<typename Functor, bool usePBC>
		class FunctionCosAngle
			: public BasicForceFieldEnergy
	{
	public:
		virtual void addE(SimulationEnvironment& env, RealNumber e[]) {
			assert (nlist % 4 == 0);
			SizePair range = env.range(nlist / 4);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*4];
				Index atom2 = lists[i*4+1];
				Index atom3 = lists[i*4+2];
				Index iPar  = lists[i*4+3];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
				}
				RealNumber cosA = calcCosAngle(delta1, delta2);
				e[0] += func(cosA, pars+iPar);
			}
		}
		virtual void addEG(SimulationEnvironment& env, RealNumber e[], XYZ gred[]) {
			assert (nlist % 4 == 0);
			SizePair range = env.range(nlist / 4);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*4];
				Index atom2 = lists[i*4+1];
				Index atom3 = lists[i*4+2];
				Index iPar  = lists[i*4+3];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
				}
				XYZ dadx[2];
				RealNumber cosAngle = calcCosAngle(delta1, delta2, dadx);
				RealNumber dedr;
				e[0] += func(cosAngle, pars+iPar, dedr);
				dadx[0] *= dedr; dadx[1] *= dedr;
				gred[atom1] += dadx[0];
				gred[atom2] -= dadx[0];
				gred[atom2] -= dadx[1];
				gred[atom3] += dadx[1];
			}
		}
		virtual void addEGH(SimulationEnvironment& env, RealNumber e[], XYZ gred[], RealNumber* hess[]) {
			assert (nlist % 4 == 0);
			SizePair range = env.range(nlist / 4);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*4];
				Index atom2 = lists[i*4+1];
				Index atom3 = lists[i*4+2];
				Index iPar  = lists[i*4+3];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
				}
				XYZ dadx[2];
				HessianItem dx2[4];
				RealNumber cosA = calcCosAngle(delta1, delta2, dadx, dx2);
				RealNumber dedr, d2edr2;
				e[0] += func(cosA, pars+iPar, dedr, d2edr2);

				HessianItem t;
				dx2[0] *= dedr; t = prod(dadx[0], dadx[0]); t *= d2edr2; dx2[0] += t;
				dx2[1] *= dedr; t = prod(dadx[0], dadx[1]); t *= d2edr2; dx2[1] += t;
				dx2[2] *= dedr; t = prod(dadx[1], dadx[0]); t *= d2edr2; dx2[2] += t;
				dx2[3] *= dedr; t = prod(dadx[1], dadx[1]); t *= d2edr2; dx2[3] += t;

				env.lock();
					addHessianItem(dx2[0], atom1, atom1, hess);

					subHessianItem(dx2[0], atom1, atom2, hess);
					subHessianItem(dx2[1], atom1, atom2, hess);

					addHessianItem(dx2[1], atom1, atom3, hess);

					subHessianItem(dx2[0], atom2, atom1, hess);
					subHessianItem(dx2[2], atom2, atom1, hess);

					addHessianItem(dx2[0], atom2, atom2, hess);
					addHessianItem(dx2[1], atom2, atom2, hess);
					addHessianItem(dx2[2], atom2, atom2, hess);
					addHessianItem(dx2[3], atom2, atom2, hess);

					subHessianItem(dx2[1], atom2, atom3, hess);
					subHessianItem(dx2[3], atom2, atom3, hess);

					addHessianItem(dx2[2], atom3, atom1, hess);

					subHessianItem(dx2[3], atom3, atom2, hess);
					subHessianItem(dx2[2], atom3, atom2, hess);

					addHessianItem(dx2[3], atom3, atom3, hess);	
				env.free();

				dadx[0] *= dedr; dadx[1] *= dedr;
				gred[atom1] += dadx[0];
				gred[atom2] -= dadx[0];
				gred[atom2] -= dadx[1];
				gred[atom3] += dadx[1];
			}
		}

		virtual void addHX(SimulationEnvironment& env, const RealNumber x[], RealNumber result[]) {
			assert (nlist % 4 == 0);
			SizePair range = env.range(nlist / 4);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*4];
				Index atom2 = lists[i*4+1];
				Index atom3 = lists[i*4+2];
				Index iPar  = lists[i*4+3];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
				}
				XYZ dadx[2];
				HessianItem dx2[4];
				RealNumber cosA = calcCosAngle(delta1, delta2, dadx, dx2);
				RealNumber dedr, d2edr2;
				func(cosA, pars+iPar, dedr, d2edr2);

				HessianItem t;
				dx2[0] *= dedr; t = prod(dadx[0], dadx[0]); t *= d2edr2; dx2[0] += t;
				dx2[1] *= dedr; t = prod(dadx[0], dadx[1]); t *= d2edr2; dx2[1] += t;
				dx2[2] *= dedr; t = prod(dadx[1], dadx[0]); t *= d2edr2; dx2[2] += t;
				dx2[3] *= dedr; t = prod(dadx[1], dadx[1]); t *= d2edr2; dx2[3] += t;

				
				addHessianItem(dx2[0], atom1, atom1, x, result);

				subHessianItem(dx2[0], atom1, atom2, x, result);
				subHessianItem(dx2[1], atom1, atom2, x, result);

				addHessianItem(dx2[1], atom1, atom3, x, result);

				subHessianItem(dx2[0], atom2, atom1, x, result);
				subHessianItem(dx2[2], atom2, atom1, x, result);

				addHessianItem(dx2[0], atom2, atom2, x, result);
				addHessianItem(dx2[1], atom2, atom2, x, result);
				addHessianItem(dx2[2], atom2, atom2, x, result);
				addHessianItem(dx2[3], atom2, atom2, x, result);

				subHessianItem(dx2[1], atom2, atom3, x, result);
				subHessianItem(dx2[3], atom2, atom3, x, result);

				addHessianItem(dx2[2], atom3, atom1, x, result);

				subHessianItem(dx2[3], atom3, atom2, x, result);
				subHessianItem(dx2[2], atom3, atom2, x, result);

				addHessianItem(dx2[3], atom3, atom3, x, result);	
			}
		}

		Functor func;
	};

	template<typename Functor, bool usePBC>
		class FunctionCosTorsion
			: public BasicForceFieldEnergy
	{
	public:
		virtual void addE(SimulationEnvironment& env, RealNumber e[]) {
			assert (nlist % 5 == 0);
			SizePair range = env.range(nlist / 5);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*5];
				Index atom2 = lists[i*5+1];
				Index atom3 = lists[i*5+2];
				Index atom4 = lists[i*5+3];
				Index iPar  = lists[i*5+4];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom4].x - coords[atom3].x, coords[atom4].y - coords[atom3].y, coords[atom4].z - coords[atom3].z);
				XYZ axis(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
					pbc->update(axis);
				}
				RealNumber cosA = calcCosTorsion(delta1, delta2, axis);
				e[0] += func(cosA, pars+iPar);
			}
		}
		virtual void addEG(SimulationEnvironment& env, RealNumber e[], XYZ gred[]) {
			assert (nlist % 5 == 0);
			SizePair range = env.range(nlist / 5);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*5];
				Index atom2 = lists[i*5+1];
				Index atom3 = lists[i*5+2];
				Index atom4 = lists[i*5+3];
				Index iPar  = lists[i*5+4];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom4].x - coords[atom3].x, coords[atom4].y - coords[atom3].y, coords[atom4].z - coords[atom3].z);
				XYZ axis(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
					pbc->update(axis);
				}
				XYZ dtdx[3];
				RealNumber cosA = calcCosTorsion(delta1, delta2, axis, dtdx);
				RealNumber dedt;
				e[0] += func(cosA, pars+iPar, dedt);
				dtdx[0] *= dedt;
				dtdx[1] *= dedt;
				dtdx[2] *= dedt;
				gred[atom1] += dtdx[0];
				gred[atom2] -= dtdx[0];
				gred[atom2] -= dtdx[2];
				gred[atom3] += dtdx[2];
				gred[atom3] -= dtdx[1];
				gred[atom4] += dtdx[1];
			}
		}

		virtual void addEGH(SimulationEnvironment& env, RealNumber e[], XYZ gred[], RealNumber* hess[]) {
			assert (nlist % 5 == 0);
			SizePair range = env.range(nlist / 5);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*5];
				Index atom2 = lists[i*5+1];
				Index atom3 = lists[i*5+2];
				Index atom4 = lists[i*5+3];
				Index iPar  = lists[i*5+4];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom4].x - coords[atom3].x, coords[atom4].y - coords[atom3].y, coords[atom4].z - coords[atom3].z);
				XYZ axis(coords[atom2].x - coords[atom3].x, coords[atom2].y - coords[atom3].y, coords[atom2].z - coords[atom3].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
					pbc->update(axis);
				}

				XYZ dx[3];
				HessianItem dx2[9];
				RealNumber cosA = calcCosTorsion(delta1, delta2, axis, dx, dx2);
				RealNumber de, de2;
				e[0] += func(cosA, pars+iPar, de, de2);

				HessianItem t;
				dx2[0] *= de; t = prod(dx[0], dx[0]);  t *= de2; dx2[0] += t;
				dx2[1] *= de; t = prod(dx[0], dx[1]);  t *= de2; dx2[1] += t;
				dx2[2] *= de; t = prod(dx[0], dx[2]);  t *= de2; dx2[2] += t;
				dx2[3] *= de; t = prod(dx[1], dx[0]);  t *= de2; dx2[3] += t;
				dx2[4] *= de; t = prod(dx[1], dx[1]);  t *= de2; dx2[4] += t;
				dx2[5] *= de; t = prod(dx[1], dx[2]);  t *= de2; dx2[5] += t;
				dx2[6] *= de; t = prod(dx[2], dx[0]);  t *= de2; dx2[6] += t;
				dx2[7] *= de; t = prod(dx[2], dx[1]);  t *= de2; dx2[7] += t;
				dx2[8] *= de; t = prod(dx[2], dx[2]);  t *= de2; dx2[8] += t;

				env.lock();
					addHessianItem(dx2[0], atom1, atom1, hess);

					subHessianItem(dx2[0], atom1, atom2, hess);
					addHessianItem(dx2[2], atom1, atom2, hess);

					subHessianItem(dx2[1], atom1, atom3, hess);
					subHessianItem(dx2[2], atom1, atom3, hess);

					addHessianItem(dx2[1], atom1, atom4, hess);

					subHessianItem(dx2[0], atom2, atom1, hess);
					addHessianItem(dx2[6], atom2, atom1, hess);

					addHessianItem(dx2[0], atom2, atom2, hess);
					subHessianItem(dx2[2], atom2, atom2, hess);
					subHessianItem(dx2[6], atom2, atom2, hess);
					addHessianItem(dx2[8], atom2, atom2, hess);

					addHessianItem(dx2[1], atom2, atom3, hess);
					addHessianItem(dx2[2], atom2, atom3, hess);
					subHessianItem(dx2[7], atom2, atom3, hess);
					subHessianItem(dx2[8], atom2, atom3, hess);
					

					subHessianItem(dx2[1], atom2, atom4, hess);
					addHessianItem(dx2[7], atom2, atom4, hess);

					subHessianItem(dx2[3], atom3, atom1, hess);
					subHessianItem(dx2[6], atom3, atom1, hess);

					addHessianItem(dx2[3], atom3, atom2, hess);
					subHessianItem(dx2[5], atom3, atom2, hess);
					addHessianItem(dx2[6], atom3, atom2, hess);
					subHessianItem(dx2[8], atom3, atom2, hess);
					

					addHessianItem(dx2[8], atom3, atom3, hess);
					addHessianItem(dx2[7], atom3, atom3, hess);
					addHessianItem(dx2[5], atom3, atom3, hess);
					addHessianItem(dx2[4], atom3, atom3, hess);

					subHessianItem(dx2[4], atom3, atom4, hess);
					subHessianItem(dx2[7], atom3, atom4, hess);
					
					addHessianItem(dx2[3], atom4, atom1, hess);

					subHessianItem(dx2[3], atom4, atom2, hess);
					addHessianItem(dx2[5], atom4, atom2, hess);

					subHessianItem(dx2[4], atom4, atom3, hess);
					subHessianItem(dx2[5], atom4, atom3, hess);

					addHessianItem(dx2[4], atom4, atom4, hess);
					
				env.free();

				dx[0] *= de;
				dx[1] *= de;
				dx[2] *= de;
				gred[atom1] += dx[0];
				gred[atom2] -= dx[0];
				gred[atom2] += dx[2];
				gred[atom3] -= dx[2];
				gred[atom3] -= dx[1];
				gred[atom4] += dx[1];
			}
		}

		virtual void addHX(SimulationEnvironment& env, const RealNumber x[], RealNumber result[]) {
			assert (nlist % 5 == 0);
			SizePair range = env.range(nlist / 5);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*5];
				Index atom2 = lists[i*5+1];
				Index atom3 = lists[i*5+2];
				Index atom4 = lists[i*5+3];
				Index iPar  = lists[i*5+4];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom4].x - coords[atom3].x, coords[atom4].y - coords[atom3].y, coords[atom4].z - coords[atom3].z);
				XYZ axis(coords[atom2].x - coords[atom3].x, coords[atom2].y - coords[atom3].y, coords[atom2].z - coords[atom3].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
					pbc->update(axis);
				}

				XYZ dx[3];
				HessianItem dx2[9];
				RealNumber cosA = calcCosTorsion(delta1, delta2, axis, dx, dx2);
				RealNumber de, de2;
				func(cosA, pars+iPar, de, de2);

				HessianItem t;
				dx2[0] *= de; t = prod(dx[0], dx[0]);  t *= de2; dx2[0] += t;
				dx2[1] *= de; t = prod(dx[0], dx[1]);  t *= de2; dx2[1] += t;
				dx2[2] *= de; t = prod(dx[0], dx[2]);  t *= de2; dx2[2] += t;
				dx2[3] *= de; t = prod(dx[1], dx[0]);  t *= de2; dx2[3] += t;
				dx2[4] *= de; t = prod(dx[1], dx[1]);  t *= de2; dx2[4] += t;
				dx2[5] *= de; t = prod(dx[1], dx[2]);  t *= de2; dx2[5] += t;
				dx2[6] *= de; t = prod(dx[2], dx[0]);  t *= de2; dx2[6] += t;
				dx2[7] *= de; t = prod(dx[2], dx[1]);  t *= de2; dx2[7] += t;
				dx2[8] *= de; t = prod(dx[2], dx[2]);  t *= de2; dx2[8] += t;

				
				addHessianItem(dx2[0], atom1, atom1, x, result);

				subHessianItem(dx2[0], atom1, atom2, x, result);
				addHessianItem(dx2[2], atom1, atom2, x, result);

				subHessianItem(dx2[1], atom1, atom3, x, result);
				subHessianItem(dx2[2], atom1, atom3, x, result);

				addHessianItem(dx2[1], atom1, atom4, x, result);

				subHessianItem(dx2[0], atom2, atom1, x, result);
				addHessianItem(dx2[6], atom2, atom1, x, result);

				addHessianItem(dx2[0], atom2, atom2, x, result);
				subHessianItem(dx2[2], atom2, atom2, x, result);
				subHessianItem(dx2[6], atom2, atom2, x, result);
				addHessianItem(dx2[8], atom2, atom2, x, result);

				addHessianItem(dx2[1], atom2, atom3, x, result);
				addHessianItem(dx2[2], atom2, atom3, x, result);
				subHessianItem(dx2[7], atom2, atom3, x, result);
				subHessianItem(dx2[8], atom2, atom3, x, result);
				

				subHessianItem(dx2[1], atom2, atom4, x, result);
				addHessianItem(dx2[7], atom2, atom4, x, result);

				subHessianItem(dx2[3], atom3, atom1, x, result);
				subHessianItem(dx2[6], atom3, atom1, x, result);

				addHessianItem(dx2[3], atom3, atom2, x, result);
				subHessianItem(dx2[5], atom3, atom2, x, result);
				addHessianItem(dx2[6], atom3, atom2, x, result);
				subHessianItem(dx2[8], atom3, atom2, x, result);
				

				addHessianItem(dx2[8], atom3, atom3, x, result);
				addHessianItem(dx2[7], atom3, atom3, x, result);
				addHessianItem(dx2[5], atom3, atom3, x, result);
				addHessianItem(dx2[4], atom3, atom3, x, result);

				subHessianItem(dx2[4], atom3, atom4, x, result);
				subHessianItem(dx2[7], atom3, atom4, x, result);
				
				addHessianItem(dx2[3], atom4, atom1, x, result);

				subHessianItem(dx2[3], atom4, atom2, x, result);
				addHessianItem(dx2[5], atom4, atom2, x, result);

				subHessianItem(dx2[4], atom4, atom3, x, result);
				subHessianItem(dx2[5], atom4, atom3, x, result);

				addHessianItem(dx2[4], atom4, atom4, x, result);
			}
		}

	public:
		Functor func;
	};

	template<typename Functor, bool usePBC>
		class FunctionTorsion
			: public BasicForceFieldEnergy
	{
	public:
		virtual void addE(SimulationEnvironment& env, RealNumber e[]) {
			assert (nlist % 5 == 0);
			SizePair range = env.range(nlist / 5);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*5];
				Index atom2 = lists[i*5+1];
				Index atom3 = lists[i*5+2];
				Index atom4 = lists[i*5+3];
				Index iPar  = lists[i*5+4];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom4].x - coords[atom3].x, coords[atom4].y - coords[atom3].y, coords[atom4].z - coords[atom3].z);
				XYZ axis(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
					pbc->update(axis);
				}
				RealNumber torsion = calcTorsion(delta1, delta2, axis);
				e[0] += func(torsion, pars+iPar);
			}
		}
		virtual void addEG(SimulationEnvironment& env, RealNumber e[], XYZ gred[]) {
			assert (nlist % 5 == 0);
			SizePair range = env.range(nlist / 5);
			for (size_t i = range.first; i != range.second; ++i) {
				Index atom1 = lists[i*5];
				Index atom2 = lists[i*5+1];
				Index atom3 = lists[i*5+2];
				Index atom4 = lists[i*5+3];
				Index iPar  = lists[i*5+4];
				XYZ delta1(coords[atom1].x - coords[atom2].x, coords[atom1].y - coords[atom2].y, coords[atom1].z - coords[atom2].z);
				XYZ delta2(coords[atom4].x - coords[atom3].x, coords[atom4].y - coords[atom3].y, coords[atom4].z - coords[atom3].z);
				XYZ axis(coords[atom3].x - coords[atom2].x, coords[atom3].y - coords[atom2].y, coords[atom3].z - coords[atom2].z);
				if (usePBC) {
					pbc->update(delta1);
					pbc->update(delta2);
					pbc->update(axis);
				}
				XYZ dtdx[3];
				RealNumber torsion = calcTorsion(delta1, delta2, axis, dtdx);
				RealNumber dedt;
				e[0] += func(torsion, pars+iPar, dedt);
				dtdx[0] *= dedt;
				dtdx[1] *= dedt;
				dtdx[2] *= dedt;
				gred[atom1] += dtdx[0];
				gred[atom2] -= dtdx[0];
				gred[atom2] -= dtdx[2];
				gred[atom3] += dtdx[2];
				gred[atom3] -= dtdx[1];
				gred[atom4] += dtdx[1];
			}
		}

		virtual void addEGH(SimulationEnvironment& env, RealNumber e[], XYZ gred[], RealNumber* hess[]) {
			// TODO
		}

		virtual void addHX(SimulationEnvironment& env, const RealNumber x[], RealNumber result[]) {
			// TODO
		}


	public:
		Functor func;
	};

	class EnergySet
	{
	public:
		explicit EnergySet();
		
		void getE(SimulationEnvironment& env, RealNumber e[]);
		void getEG(SimulationEnvironment& env, RealNumber e[], XYZ gred[]);
		void getHX(SimulationEnvironment& env, const RealNumber x[], RealNumber result[]);
		void getEGH(SimulationEnvironment& env, RealNumber e[], XYZ gred[], RealNumber* hess[]);

		void addE(SimulationEnvironment& env, RealNumber e[]);
		void addEG(SimulationEnvironment& env, RealNumber e[], XYZ gred[]);
		void addHX(SimulationEnvironment& env, const RealNumber x[], RealNumber result[]);
		void addEGH(SimulationEnvironment& env, RealNumber e[], XYZ gred[], RealNumber* hess[]);

		void start(SimulationEnvironment& env);
		void finish(SimulationEnvironment& env);
		void finalize(SimulationEnvironment& env);

		Index getRank() const {
			Index result = 0;
			for (std::vector<EnergyFunction*>::const_iterator
				it = funcs.begin(); it != funcs.end(); ++it)
			{
				result += (*it)->rank;
			}
			return result;
		}

		std::vector<EnergyFunction*> funcs;
		Index nAtoms;

	private:
		std::vector<RealNumber> eBuffer;	// Buffer for energy
		std::vector<XYZ> gBuffer;			// Buffer for forces
		size_t rank;
	};

	BasicForceFieldEnergy* createValenceEnergyFunction(const char szName[], const PBC* pbc);

	

}

#endif
